In [3]:
# first: load simulation results
import mutils.io as mio
import numpy as np
import pylab as pl
class mystruct(object):
pass
steps = mio.mload('../data/vpp_sim.dat')
#create directory "anim" if it does not exist
!mkdir -p anim
#set image resolution
pl.rcParams['savefig.dpi'] = 100 # adapt to whatever resolution the image file should have
# define sampling time: 50ms (20fps) -> using 20fps in video gives real-time
ts = .025
t_last = 0
n_step = 0
for s in steps:
# resample t, y, Force
t = arange(t_last, s.t[-1] , ts)
s.y = vstack([interp(t, s.t, s.y[:, k]) for k in arange(s.y.shape[1])]).T
# forces are more complicated: leg1 is always trailing leg! -> switch at TO
n_step += 1
F1, F2 = [], []
if mod(n_step, 2):
idx1 = [0, 1, 4]
idx2 = [2, 3, 5]
else:
idx1 = [2, 3, 5]
idx2 = [0, 1, 4]
F1.append( vstack([interp(t, s.t, s.Forces[:, k]) for k in idx1]).T )
F2.append( vstack([interp(t, s.t, s.Forces[:, k]) for k in idx2]).T )
s.F1 = vstack(F1)
s.F2 = vstack(F2)
#s.Forces = vstack([interp(t, s.t, s.Forces[:, k]) for k in arange(s.Forces.shape[1])]).T
t_last = t[-1]
s.t = t
In [4]:
# verify data
# *NOTE* this cell is required for later (total force and total time)
t_tot = hstack([s.t for s in steps])
x_tot = hstack([s.y[:,0] for s in steps])
y_tot = hstack([s.y[:,1] for s in steps])
Fx1_tot = hstack([s.F1[:, 0] for s in steps])
Fy1_tot = hstack([s.F1[:, 1] for s in steps])
tau1_tot = hstack([s.F1[:, 2] for s in steps])
Fx2_tot = hstack([s.F2[:, 0] for s in steps])
Fy2_tot = hstack([s.F2[:, 1] for s in steps])
tau2_tot = hstack([s.F2[:, 2] for s in steps])
figure()
plot(t_tot, y_tot, 'r')
plot(x_tot, y_tot, 'b.-')
plot(t_tot, Fy1_tot, 'r.-')
plot(t_tot, Fy2_tot, 'b.-')
#print [(s.x_foot1, s.x_foot2) for s in steps]
Out[4]:
In [5]:
# Initialize the frame
#initialize the plotting frame
import mutils.plotting as mplt
reload(mplt)
fig = mplt.mfig('a5', 'my figure')
fig.set_layout(mplt.layouts['plain']) # plain format: suitable for videos
# erase ticks
ax = fig.axes[0]
ax.set_frame_on(False)
ax.set_xticks([])
ax.set_yticks([])
ax.axis('equal')
scale_factor = .0175
# a5 format: 148 x 210 mm
# average CoM height ~ 1.1 m
ax.set_ylim([1.1 - scale_factor * 148. / 2,
1.1 + scale_factor * 148. / 2.])
# model should be left of center of frame
ax.set_xlim([1. - scale_factor * 210. / 2.,
1. + scale_factor * 210. / 2.])
ax_f = fig.fig.add_axes([.75, 0.125, .25, .25], frameon=False)
ax_f.set_xticks([])
ax_f.set_yticks([])
ax_f.plot(t_tot, Fy1_tot, color='#47af23', linewidth=3.5)
ax_f.plot(t_tot, Fy2_tot, color='#2347af', linewidth=3.5)
ax_f.set_xlim([-.5, .5])
plt_ylim = (-20, 1.1 * max(hstack([Fy1_tot, Fy2_tot])))
ax_f.set_ylim(plt_ylim)
Out[5]:
In [6]:
# initialize the model (patches and handles)
# vertical line on "time" plot (is a separate frame!)
h_tline = ax_f.plot([0,0], plt_ylim, '.-', color='#cfcfcf')[0]
# bottom
h_bottom = ax.fill([-1, 3, 3, -1],[0, 0, -.1, -.1], edgecolor='#ff0000', facecolor='#000000')
# vpp body
body_patch = np.array([
[0, .01, .04, .06, .07, .06, .04, .01, 0, -.01, -.04, -.06, -.07, -.06, -.04, -.01],
[-.5, -.495, -.45, -.35, 0, .35, .45, .495, .5, .495, .45, .35, 0, -.35, -.45, -.495] ])
h_body = ax.fill(body_patch[0, :], body_patch[1, :], edgecolor='#001133', facecolor='#969696', zorder=1)[0]
# spring leg
spring_patch = np.array([
[0, 0, -.04, .04, -.04, .04, -.04, .04, 0, 0],
[1, .62, .6, .56 , .52, .48, .44, .40, .38, 0]])
h_leg1 = ax.plot(spring_patch[0, :], spring_patch[1, :], color='#45ca65', linewidth=4, solid_capstyle='butt')[0]
h_leg2 = ax.plot(spring_patch[0, :] + .2, spring_patch[1, :], color='#4565ca', linewidth=4, solid_capstyle='butt')[0]
# vpp, hip and com patches
vpp_patch = .02 * np.array([sin(linspace(0, 2. * np.pi, 100)),
cos(linspace(0, 2. * np.pi, 100)) ])
h_vpp1 = ax.fill(vpp_patch[0, :], vpp_patch[1, :], edgecolor='#001133', facecolor='#764312', zorder=3)[0]
h_vpp2 = ax.fill(vpp_patch[0, :], vpp_patch[1, :], edgecolor='#001133', facecolor='#761243', zorder=3)[0]
hip_patch = .02 * np.array([sin(linspace(0, 2. * np.pi, 100)),
cos(linspace(0, 2. * np.pi, 100)) ])
h_hip = ax.fill(hip_patch[0, :], hip_patch[1, :] + 1, edgecolor='#001133', facecolor='#346512', zorder=3)[0]
#com-patch
com1_patch = .022 * np.array([sin(linspace(0, 2. * np.pi, 100)),
cos(linspace(0, 2. * np.pi, 100)) ])
h_com1 = ax.fill(com1_patch[0, :], com1_patch[1, :], edgecolor='#000000', facecolor='#000000', zorder=5)[0]
com2_patch = .02 * np.array([sin(hstack([linspace(0, .5 * np.pi, 25), linspace(1.5 * pi, pi, 25),])),
cos(hstack([linspace(0, .5 * np.pi, 25), linspace(1.5 * pi, pi, 25),])) ])
h_com2 = ax.fill(com2_patch[0, :], com2_patch[1, :], edgecolor='#ffffff', facecolor='#ffffff', zorder=6)[0]
In [7]:
# draw the figure - draw each frame
# recall the figure to get output!
figure('my figure')
def rotmat(phi):
""" a 2d rotation matrix """
return array([[cos(phi), -sin(phi)],
[sin(phi), cos(phi)]])
counter = 0
nstep = 0
for s in steps:
# update body
nstep += 1
for idx, t in enumerate(s.t):
com_x, com_y, phi = s.y[idx, :3]
# set reference positions. currently: (com_x, 0)
x_ref, y_ref = com_x, 0
h_tline.set_data([t, t], plt_ylim)
ax_f.set_xlim([-.5 + t, .5 + t])
# body
p_body = dot(rotmat(phi), body_patch)
p_body[0, :] += com_x - x_ref
p_body[1, :] += com_y - y_ref
h_body.set_xy(p_body.T)
# CoM
p_com1 = com1_patch.copy()
p_com1[0, :] += com_x - x_ref
p_com1[1, :] += com_y - y_ref
h_com1.set_xy(p_com1.T)
p_com2 = com2_patch.copy()
p_com2[0, :] += com_x - x_ref
p_com2[1, :] += com_y - y_ref
h_com2.set_xy(p_com2.T)
# VPPs
p_vpp = vpp_patch.copy()
p_vpp[0, :] += com_x - x_ref - s.P['r_vpp1'] * sin(s.P['a_vpp1'] + phi)
p_vpp[1, :] += com_y - y_ref + s.P['r_vpp1'] * cos(s.P['a_vpp1'] + phi)
h_vpp1.set_xy(p_vpp.T)
p_vpp = vpp_patch.copy()
p_vpp[0, :] += com_x - x_ref - s.P['r_vpp2'] * sin(s.P['a_vpp2'] + phi)
p_vpp[1, :] += com_y - y_ref + s.P['r_vpp2'] * cos(s.P['a_vpp2'] + phi)
h_vpp2.set_xy(p_vpp.T)
# hip
hip_x = com_x + s.P['r_hip'] * sin(phi) - x_ref
hip_y = com_y - s.P['r_hip'] * cos(phi) - y_ref
p_hip = hip_patch.copy()
p_hip[0, :] += hip_x
p_hip[1, :] += hip_y
h_hip.set_xy(p_hip.T)
odd_step = mod(nstep, 2)
# legs
# compute angle of leg 1
xf1 = s.x_foot2 - x_ref
contact1 = t <= s.t_to
if contact1:
a1 = arctan((xf1 - hip_x) / hip_y)
l1 = sqrt((xf1 - hip_x)**2 + hip_y**2)
else:
a1 = pi / 2. - s.P['legpars1']['alpha']
l1 = s.P['legpars1']['l0']
# adjust xf1 (footpoint)
xf1 = hip_x + l1 * sin(a1)
p_leg1 = reduce(dot, [rotmat(a1), array([[1, 0],[0, l1]]), spring_patch])
p_leg1[0, :] += xf1
if not contact1:
p_leg1[1, :] += hip_y - l1 * cos(a1)
# *NOTE*: updating the patch is done below!
# compute angle of leg 2
xf2 = s.x_foot1 - x_ref
contact2 = t >= s.t_td
if contact2:
a2 = arctan((xf2 - hip_x) / hip_y)
l2 = sqrt((xf2 - hip_x)**2 + hip_y**2)
else:
a2 = pi / 2. - s.P['legpars2']['alpha']
l2 = s.P['legpars2']['l0']
# adjust xf2 (footpoint)
xf2 = hip_x + l2 * sin(a2)
p_leg2 = reduce(dot, [rotmat(a2), array([[1, 0],[0, l2]]), spring_patch])
p_leg2[0, :] += xf2
if not contact2:
p_leg2[1, :] += hip_y - l2 * cos(a2)
# switch legs in even and odd steps
if odd_step:
h_leg2.set_data(p_leg2[0, :], p_leg2[1, :])
h_leg1.set_data(p_leg1[0, :], p_leg1[1, :])
else:
h_leg1.set_data(p_leg2[0, :], p_leg2[1, :])
h_leg2.set_data(p_leg1[0, :], p_leg1[1, :])
draw()
fig.fig.savefig('anim/fig_%04d.png' % counter)
#savefig('anim/fig_%04d.png' % counter, res=200)
counter += 1
In [8]:
# convert into movie (ogg):
# old version (raises deprecation warning)
#!ffmpeg -f image2 -i anim/fig%03d.png anim.mp4
# new version
# -y: overwrite existing file, -r 20: 20 fps, -f image2 : filetype image,
# -i anim/fig%03d.png: input files, all that start with anim/fig, have 3 numbers, and end with .png
# anim/anim.ogg - the output file name
# *NOTE* 'ogg' is web-compatible. If you want another format, just change the suffix (e.g. '.mp4')
!avconv -y -r 15 -f image2 -i anim/fig_%04d.png -vb 600k anim/vpp.ogg
!rm anim/*.png
In [9]:
# display the video inline!
# note: this needs a recent browser... with HTML5 and the option of OGG-video playback
from IPython.core.display import HTML
video = open('anim/vpp.ogg', 'rb').read()
video_encoded = video.encode('base64') # encode the video in HTML-conform style
video_tag = ('<video controls type="video/ogg" '
+ 'alt="VPP walking model" src="data:video/ogg;base64,{0}">').format(video_encoded)
HTML(data = video_tag)
Out[9]:
In [18]: